#include <stdio.h>
#include "Nano100Series.h"
#include "Mpu6500.h"
#include "I2C1.h"

#define MIN(x,y) (x < y ? x : y)

#define MPU6500_I2C_ADDR		0x68
#define MPU6500_PID				0x70

enum mpu6500_reg_addr_t {
	MPU6500_REG_ADDR_SELF_TEST_X_GYRO = 0,	// 0
	MPU6500_REG_ADDR_MPU6500_REG_ADDR_SELF_TEST_Y_GYRO,
	MPU6500_REG_ADDR_MPU6500_REG_ADDR_SELF_TEST_Z_GYRO,
	MPU6500_REG_ADDR_RESERVED3,
	MPU6500_REG_ADDR_RESERVED4,
	MPU6500_REG_ADDR_RESERVED5,
	MPU6500_REG_ADDR_RESERVED6,
	MPU6500_REG_ADDR_RESERVED7,
	MPU6500_REG_ADDR_RESERVED8,
	MPU6500_REG_ADDR_RESERVED9,
	MPU6500_REG_ADDR_RESERVED10,			// 10
	MPU6500_REG_ADDR_RESERVED11,
	MPU6500_REG_ADDR_RESERVED12,
	MPU6500_REG_ADDR_SELF_TEST_X_ACCEL,
	MPU6500_REG_ADDR_SELF_TEST_Y_ACCEL,
	MPU6500_REG_ADDR_SELF_TEST_Z_ACCEL,
	MPU6500_REG_ADDR_RESERVED16,
	MPU6500_REG_ADDR_RESERVED17,
	MPU6500_REG_ADDR_RESERVED18,
	MPU6500_REG_ADDR_XG_OFFSET_H,
	MPU6500_REG_ADDR_XG_OFFSET_L,			// 20
	MPU6500_REG_ADDR_YG_OFFSET_H,
	MPU6500_REG_ADDR_YG_OFFSET_L,
	MPU6500_REG_ADDR_ZG_OFFSET_H,
	MPU6500_REG_ADDR_ZG_OFFSET_L,
	MPU6500_REG_ADDR_SMPLRT_DIV,
	MPU6500_REG_ADDR_CONFIG,
	MPU6500_REG_ADDR_GYRO_CONFIG,
	MPU6500_REG_ADDR_ACCEL_CONFIG,
	MPU6500_REG_ADDR_ACCEL_CONFIG2,
	MPU6500_REG_ADDR_LP_ACCEL_ODR,			// 30
	MPU6500_REG_ADDR_WOM_THR,
	MPU6500_REG_ADDR_RESERVED32,
	MPU6500_REG_ADDR_RESERVED33,
	MPU6500_REG_ADDR_RESERVED34,
	MPU6500_REG_ADDR_FIFO_EN,
	MPU6500_REG_ADDR_I2C_MST_CTRL,
	MPU6500_REG_ADDR_I2C_SLV0_ADDR,
	MPU6500_REG_ADDR_I2C_SLV0_REG,
	MPU6500_REG_ADDR_I2C_SLV0_CTRL,
	MPU6500_REG_ADDR_I2C_SLV1_ADDR,			// 40
	MPU6500_REG_ADDR_I2C_SLV1_REG,
	MPU6500_REG_ADDR_I2C_SLV1_CTRL,
	MPU6500_REG_ADDR_I2C_SLV2_ADDR,
	MPU6500_REG_ADDR_I2C_SLV2_REG,
	MPU6500_REG_ADDR_I2C_SLV2_CTRL,
	MPU6500_REG_ADDR_I2C_SLV3_ADDR,
	MPU6500_REG_ADDR_I2C_SLV3_REG,
	MPU6500_REG_ADDR_I2C_SLV3_CTRL,
	MPU6500_REG_ADDR_I2C_SLV4_ADDR,
	MPU6500_REG_ADDR_I2C_SLV4_REG,			// 50
	MPU6500_REG_ADDR_I2C_SLV4_DO,
	MPU6500_REG_ADDR_I2C_SLV4_CTRL,
	MPU6500_REG_ADDR_I2C_SLV4_DI,
	MPU6500_REG_ADDR_I2C_MST_STATUS,
	MPU6500_REG_ADDR_INT_PIN_CFG,
	MPU6500_REG_ADDR_INT_ENABLE,
	MPU6500_REG_ADDR_RESERVED57,
	MPU6500_REG_ADDR_INT_STATUS,
	MPU6500_REG_ADDR_ACCEL_XOUT_H,
	MPU6500_REG_ADDR_ACCEL_XOUT_L,			// 60
	MPU6500_REG_ADDR_ACCEL_YOUT_H,
	MPU6500_REG_ADDR_ACCEL_YOUT_L,
	MPU6500_REG_ADDR_ACCEL_ZOUT_H,
	MPU6500_REG_ADDR_ACCEL_ZOUT_L,
	MPU6500_REG_ADDR_TEMP_OUT_H,
	MPU6500_REG_ADDR_TEMP_OUT_L,
	MPU6500_REG_ADDR_GYRO_XOUT_H,
	MPU6500_REG_ADDR_GYRO_XOUT_L,
	MPU6500_REG_ADDR_GYRO_YOUT_H,
	MPU6500_REG_ADDR_GYRO_YOUT_L,			// 70
	MPU6500_REG_ADDR_GYRO_ZOUT_H,
	MPU6500_REG_ADDR_GYRO_ZOUT_L,
	MPU6500_REG_ADDR_EXT_SENS_DATA_00,
	MPU6500_REG_ADDR_EXT_SENS_DATA_01,
	MPU6500_REG_ADDR_EXT_SENS_DATA_02,
	MPU6500_REG_ADDR_EXT_SENS_DATA_03,
	MPU6500_REG_ADDR_EXT_SENS_DATA_04,
	MPU6500_REG_ADDR_EXT_SENS_DATA_05,
	MPU6500_REG_ADDR_EXT_SENS_DATA_06,
	MPU6500_REG_ADDR_EXT_SENS_DATA_07,		// 80
	MPU6500_REG_ADDR_EXT_SENS_DATA_08,
	MPU6500_REG_ADDR_EXT_SENS_DATA_09,
	MPU6500_REG_ADDR_EXT_SENS_DATA_10,
	MPU6500_REG_ADDR_EXT_SENS_DATA_11,
	MPU6500_REG_ADDR_EXT_SENS_DATA_12,
	MPU6500_REG_ADDR_EXT_SENS_DATA_13,
	MPU6500_REG_ADDR_EXT_SENS_DATA_14,
	MPU6500_REG_ADDR_EXT_SENS_DATA_15,
	MPU6500_REG_ADDR_EXT_SENS_DATA_16,
	MPU6500_REG_ADDR_EXT_SENS_DATA_17,		// 90
	MPU6500_REG_ADDR_EXT_SENS_DATA_18,
	MPU6500_REG_ADDR_EXT_SENS_DATA_19,
	MPU6500_REG_ADDR_EXT_SENS_DATA_20,
	MPU6500_REG_ADDR_EXT_SENS_DATA_21,
	MPU6500_REG_ADDR_EXT_SENS_DATA_22,
	MPU6500_REG_ADDR_EXT_SENS_DATA_23,
	MPU6500_REG_ADDR_RESERVED97,
	MPU6500_REG_ADDR_RESERVED98,
	MPU6500_REG_ADDR_I2C_SLV0_DO,
	MPU6500_REG_ADDR_I2C_SLV1_DO,			// 100
	MPU6500_REG_ADDR_I2C_SLV2_DO,
	MPU6500_REG_ADDR_I2C_SLV3_DO,
	MPU6500_REG_ADDR_I2C_MST_DELAY_CTRL,
	MPU6500_REG_ADDR_SIGNAL_PATH_RESET,
	MPU6500_REG_ADDR_ACCEL_INTEL_CTRL,
	MPU6500_REG_ADDR_USER_CTRL,
	MPU6500_REG_ADDR_PWR_MGMT_1,
	MPU6500_REG_ADDR_PWR_MGMT_2,
	MPU6500_REG_ADDR_RESERVED109,
	MPU6500_REG_ADDR_RESERVED,				// 110
	MPU6500_REG_ADDR_RESERVED111,
	MPU6500_REG_ADDR_RESERVED112,
	MPU6500_REG_ADDR_RESERVED113,
	MPU6500_REG_ADDR_FIFO_COUNT_H,
	MPU6500_REG_ADDR_FIFO_COUNT_L,
	MPU6500_REG_ADDR_FIFO_R_W,
	MPU6500_REG_ADDR_WHO_AM_I,
	MPU6500_REG_ADDR_RESERVED118,
	MPU6500_REG_ADDR_XA_OFFSET_H,
	MPU6500_REG_ADDR_XA_OFFSET_L,			// 120
	MPU6500_REG_ADDR_RESERVED121,
	MPU6500_REG_ADDR_YA_OFFSET_H,
	MPU6500_REG_ADDR_YA_OFFSET_L,
	MPU6500_REG_ADDR_RESERVED124,
	MPU6500_REG_ADDR_ZA_OFFSET_H,
	MPU6500_REG_ADDR_ZA_OFFSET_L,
};

static int is_mpu6500_exist = 0;

static int Mpu6500Write8(uint8_t reg_addr, uint8_t reg_val)
{
	struct i2c_msg_t i2c_msg;
	int ret;
	uint8_t buf[2];

	buf[0] = reg_addr;
	buf[1] = reg_val;

	i2c_msg.addr = MPU6500_I2C_ADDR;
	i2c_msg.tx_buf = buf;
	i2c_msg.tx_to_write = 2;
	i2c_msg.tx_write = 0;
	i2c_msg.rx_buf = NULL;
	i2c_msg.rx_to_read = 0;
	i2c_msg.rx_read = 0;

	ret = I2c1Xfer(&i2c_msg);
	if (ret < 0) {
		return 0;
	} else {
		return i2c_msg.tx_write;
	}
}

static int Mpu6500Read(uint8_t reg_addr, uint8_t *reg_val, uint32_t n)
{
	struct i2c_msg_t i2c_msg;
	int ret;
	uint8_t buf[1];

	buf[0] = reg_addr;

	i2c_msg.addr = MPU6500_I2C_ADDR;
	i2c_msg.tx_buf = buf;
	i2c_msg.tx_to_write = 1;
	i2c_msg.tx_write = 0;
	i2c_msg.rx_buf = reg_val;
	i2c_msg.rx_to_read = n;
	i2c_msg.rx_read = 0;

	ret = I2c1Xfer(&i2c_msg);
	if (ret < 0) {
		return 0;
	} else {
		return i2c_msg.rx_read;
	}
}

static int Mpu6500Read8(uint8_t reg_addr, uint8_t *reg_val)
{
	return Mpu6500Read(reg_addr, reg_val, 1);
}

void Mpu6500Init()
{
	uint8_t reg_val;
	int ret;

	reg_val = 0;
	ret = Mpu6500Read8(MPU6500_REG_ADDR_WHO_AM_I, &reg_val);
	if ((ret == 0) || (reg_val != MPU6500_PID)) {
		is_mpu6500_exist = 0;
		LogDebug("can not find sensor mpu6500\n");
	} else {
		is_mpu6500_exist = 1;
		LogDebug("sensor id : 0x%02X\n", reg_val);
	}

	if (!is_mpu6500_exist) {
		return;
	}

	Mpu6500Write8(MPU6500_REG_ADDR_PWR_MGMT_1, 0x10);
	DelayMs(100);
	Mpu6500Write8(MPU6500_REG_ADDR_PWR_MGMT_1, 0x01);
	Mpu6500Write8(MPU6500_REG_ADDR_PWR_MGMT_2, 0x00);
	Mpu6500Write8(MPU6500_REG_ADDR_SMPLRT_DIV, 0x00);
	Mpu6500Write8(MPU6500_REG_ADDR_CONFIG, (0 < 0)); 					// DLPF_CFG[2:0]
	Mpu6500Write8(MPU6500_REG_ADDR_GYRO_CONFIG, (1 << 3) | (0 << 0)); 	// GYRO_FS_SEL[1:0] FCHOICE_B[1:0]
	Mpu6500Write8(MPU6500_REG_ADDR_ACCEL_CONFIG, (0 << 3));				// ACCEL_FS_SEL[1:0]
	Mpu6500Write8(MPU6500_REG_ADDR_ACCEL_CONFIG2, (0 << 3) | (0 << 0));	// ACCEL_FCHOICE_B A_DLPF_CFG
}

void Mpu6500GetAcc(uint8_t *buf, uint32_t n)
{
	int ret;
	uint8_t tmp[6] = {0};

	ret = Mpu6500Read(MPU6500_REG_ADDR_ACCEL_XOUT_H, tmp, 6);
	if (ret < 6) {
		return;
	}

	buf[0] = tmp[1];
	buf[1] = tmp[0];
	buf[2] = tmp[3];
	buf[3] = tmp[2];
	buf[4] = tmp[5];
	buf[5] = tmp[4];
}

void Mpu6500GetGyro(uint8_t *buf, uint32_t n)
{
	int ret;
	uint8_t tmp[6] = {0};

	ret = Mpu6500Read(MPU6500_REG_ADDR_GYRO_XOUT_H, tmp, 6);
	if (ret < 6) {
		return;
	}

	buf[0] = tmp[1];
	buf[1] = tmp[0];
	buf[2] = tmp[3];
	buf[3] = tmp[2];
	buf[4] = tmp[5];
	buf[5] = tmp[4];
}
